#include <ServoRate.h>

void servoInit( struct TsServo & tsServo, int nPin, unsigned long nMaxServoSpeedInMicrosPerDegree, unsigned int nInitialPosition, unsigned int nInitialRateInDegPerSec )
{
  PT_INIT( &tsServo.pt );
  tsServo.nCurPosition = nInitialPosition;
  tsServo.nTargetPosition = tsServo.nCurPosition;
  tsServo.nMaxServoSpeedInMicrosPerDegree = nMaxServoSpeedInMicrosPerDegree;
  tsServo.nMoveRateInDegPerSec = nInitialRateInDegPerSec;
  tsServo.nLastMoveTimestampInMicros = micros();
  tsServo.obj.attach( nPin );
  tsServo.obj.write( nInitialPosition );
}

unsigned int servoGetPosition( struct TsServo & tsServo )
{
  unsigned int nReturn = 0;

  nReturn = tsServo.nCurPosition;

  return nReturn;
}

void servoSetPosition( struct TsServo & tsServo, int nTargetPosition, int nMoveRateInDegPerSec )
{
  tsServo.nMoveRateInDegPerSec = nMoveRateInDegPerSec;
  tsServo.nTargetPosition = nTargetPosition;
}

boolean servoIsMoving( struct TsServo &tsServo )
{
  return ( tsServo.nTargetPosition != tsServo.nCurPosition );
}

PT_THREAD( threadMicroDelay( struct TsMicroDelay & tsMicroDelay, unsigned long nDelayMicros ) )
{
  PT_BEGIN(&tsMicroDelay.pt)
  ;

  if ( nDelayMicros > 0 )
  {
    // this only runs when thread begins
    tsMicroDelay.nMicrosStop = micros() + nDelayMicros;

    PT_WAIT_UNTIL( &tsMicroDelay.pt, ( micros() >= tsMicroDelay.nMicrosStop ) );
  }

PT_END(&tsMicroDelay.pt);
return PT_ENDED;
}

// Local variables problematic in protothreads so Imade this macro to compute next position.
#define __SERVO_POS_NEXT(servo, nDiffFromTarget) (servo.nCurPosition) + ( ((nDiffFromTarget) > 0) ? 1 : -1 )
#define __SERVO_DELAY_MICROS(servo) ( ( (servo.nMoveRateInDegPerSec) > 0 ) ? 1000000 / (servo.nMoveRateInDegPerSec) : 0 )
#define __SERVO_ELAPSED_TIME(servo) ( micros() - servo.nLastMoveTimestampInMicros ) 
#define __SERVO_DELAY_MICROS_LESS_ELAPSED_TIME(servo) ( (__SERVO_DELAY_MICROS(servo) > __SERVO_ELAPSED_TIME(servo)) ? (__SERVO_DELAY_MICROS(servo) - __SERVO_ELAPSED_TIME(servo)) : 0  ) 

PT_THREAD( threadServoController( struct TsServo & tsServo ) )
{
  int nDiff = (int) tsServo.nTargetPosition - (int) tsServo.nCurPosition;

  PT_BEGIN(&tsServo.pt);

  if ( nDiff != 0 )
  {
    // tell servo to move 1 degree to next position
    tsServo.nLastMoveTimestampInMicros = micros();
    tsServo.obj.write( __SERVO_POS_NEXT(tsServo, nDiff) );

    // wait for the servo to move 1 degree (so, wait tsServo.nMaxServoSpeedInMicrosPerDegree microseconds)
    PT_SPAWN( &tsServo.pt, &tsServo.tsMicroDelay.pt, threadMicroDelay(tsServo.tsMicroDelay, tsServo.nMaxServoSpeedInMicrosPerDegree) );

    // servo has had time to get to next degree, so update the current position in state structure
    tsServo.nCurPosition = __SERVO_POS_NEXT(tsServo, nDiff);

    // delay some to implement the move rate.  So this is (1000000 / tsServo.nMoveRateInDegPerSec) , but we
    // have to subtract off the time is has taken us to move 1 degree, which is
    //  micros() - tsServo.nLastMoveTimestampInMicros.  And we only need to do this if we are not at
    // the target position and the move rate is > 0.  Move rate == 0 means full speed (no delay).

    if ( ( tsServo.nMoveRateInDegPerSec > 0 ) && ( tsServo.nTargetPosition != tsServo.nCurPosition ) )
    {
  //      lcd.setCursor(10,1);
  //      lcd.print(__SERVO_DELAY_MICROS_LESS_ELAPSED_TIME(tsServo));

      PT_SPAWN( &tsServo.pt, &tsServo.tsMicroDelay.pt, threadMicroDelay(tsServo.tsMicroDelay, __SERVO_DELAY_MICROS_LESS_ELAPSED_TIME(tsServo) ) );
  //      delay(__SERVO_DELAY_MICROS_LESS_ELAPSED_TIME(tsServo) / 1000);
  //      delayMicroseconds(__SERVO_DELAY_MICROS_LESS_ELAPSED_TIME(tsServo) % 1000);
    }
  }

  PT_END(&tsServo.pt);
  return PT_ENDED;
}
